/*
 * ElectricPropulsion.cpp
 *
 *  Created on: Apr 12, 2010
 *      Author: andrew
 */

#include "ElectricPropulsion.h"
#include <cmath>
#include <algorithm>
#include <iostream>
#include <fstream>

using namespace std;

ElectricPropulsion::ElectricPropulsion(Atmosphere* a) {
	atm = a;
	T = 0;
	Q = 0;
	Vmin = 0;
	Vmax = 20.5;

	loadTable();
}

ElectricPropulsion::~ElectricPropulsion(){
	/*if(Ttable!=NULL) {
		delete Ttable;
	}
	if(Mtable==NULL) {
		delete Mtable;
	}*/
}

double ElectricPropulsion::getThrust(double V, double alt, double throttle){
	interpFromTable(V,throttle);
	return T;
}

double ElectricPropulsion::getTorque(double V, double alt, double throttle){
	interpFromTable(V,throttle);
	return Q;
}

void ElectricPropulsion::interpFromTable(double speed, double throttle){

	if (speed < Vmin || speed > Vmax || throttle < 0 || throttle > 1){
		cout << "Error: out of interpolation bounds for thrust table" << endl;
		T = 0;
		Q = 0;
		return;
	}

	double Vp,Vm,tp,tm;
	int i,j;

	double V[nPtsV];
	double dV = (Vmax-Vmin)/(nPtsV-1);
	for(i = 0; i < nPtsV; i++){
		V[i] = Vmin + i*dV;
	}
	V[nPtsV-1] = Vmax;

	double t[nPtsT];
	double dt = 1.0/(nPtsT-1);
	for(i = 0; i < nPtsT; i++){
		t[i] = i*dt;
	}
	t[nPtsT-1] = 1.0;

	for (i = 0; i < nPtsV; i++){
		if (speed <= V[i]){
			Vp = V[i];
			Vm = Vp - dV;
			break;
		}
	}

	for (j = 0; j < nPtsT; j++){
		if (throttle <= t[j]){
			tp = t[j];
			tm = tp - dt;
			break;
		}
	}

	int tmp = i;
	i = j;
	j = tmp;

	// need to handle a few edge cases
	if (i == 0 && j == 0){
		T = (*Ttable)(i,j);
		Q = (*Mtable)(i,j);

	} else if (i == 0){
		double Ttm = (*Ttable)(i,j-1);
		double Ttp = (*Ttable)(i,j);

		double Qtm = (*Mtable)(i,j-1);
		double Qtp = (*Mtable)(i,j);

		T = interp(tm,Ttm,tp,Ttp,throttle);
		Q = interp(tm,Qtm,tp,Qtp,throttle);

	} else if (j == 0){
		double TVm = (*Ttable)(i-1,j);
		double TVp = (*Ttable)(i,j);

		double QVm = (*Mtable)(i-1,j);
		double QVp = (*Mtable)(i,j);

	 	T = interp(Vm,TVm,Vp,TVp,speed);
		Q = interp(Vm,QVm,Vp,QVp,speed);
	} else{

		// pull out corresponding data from the table
		double TVmtm = (*Ttable)(i-1,j-1);
		double TVptm = (*Ttable)(i,j-1);
		double TVmtp = (*Ttable)(i-1,j);
		double TVptp = (*Ttable)(i,j);

		double QVmtm = (*Mtable)(i-1,j-1);
		double QVptm = (*Mtable)(i,j-1);
		double QVmtp = (*Mtable)(i-1,j);
		double QVptp = (*Mtable)(i,j);

		// 2-way interpolation
		double Ttm = interp(Vm,TVmtm,Vp,TVptm,speed);
		double Ttp = interp(Vm,TVmtp,Vp,TVptp,speed);

		double Qtm = interp(Vm,QVmtm,Vp,QVptm,speed);
		double Qtp = interp(Vm,QVmtp,Vp,QVptp,speed);

		T = interp(tm,Ttm,tp,Ttp,throttle);
		Q = interp(tm,Qtm,tp,Qtp,throttle);
	}
}

double ElectricPropulsion::interp(double x1, double y1, double x2, double y2, double x){
	return y1 + (y2-y1)/(x2-x1)*(x-x1);
}

void ElectricPropulsion::loadTable(){
	ifstream Tfile ("thrustTable.txt");
	ifstream Mfile ("momentTable.txt");
	int i = 0, j = 0;

	if (Tfile.is_open() && Mfile.is_open()){
		Tfile >> Vmin;
		Tfile >> Vmax;
		Tfile >> nPtsV;
		Tfile >> nPtsT;
		Ttable = new Matrix(nPtsT+1,nPtsV+1);
		Mtable = new Matrix(nPtsT+1,nPtsV+1);

		while (! Tfile.eof() ){
			Tfile >> (*Ttable)(i,j);
			Mfile >> (*Mtable)(i,j++);
			if (j == nPtsV){
				i++;
				j = 0;
			}
		}
		Tfile.close();
		Mfile.close();
		cout << "Thrust and moment propulsion tables loaded" << endl;
	}
}


//void ElectricPropulsion::generateTable(double Vmin, double Vmax, int nPts){
//	if (nPts < 2){
//		cout << "nPts must be >= 2";
//		return;
//	}
//
//	double V[nPts];
//	double dV = (Vmax-Vmin)/(nPts-1);
//	for(int i = 0; i < nPts; i++){
//		V[i] = Vmin + i*dV;
//	}
//	V[nPts-1] = Vmax;
//
//	double t[nPts];
//	double dt = 1.0/(nPts-1);
//	for(int i = 0; i < nPts; i++){
//		t[i] = i*dt;
//	}
//	t[nPts-1] = 1.0;
//
//
//	ofstream Tfile ("thrustTable.txt");
//	ofstream Mfile ("momentTable.txt");
//	if (Tfile.is_open() && Mfile.is_open())
//	{
//		Tfile << Vmin << "\t" << Vmax << "\t" << nPts << endl;
//
//		for(int i = 0; i < nPts; i++){
//			for (int j = 0; j < nPts; j++){
//				compute(V[i],0,t[j]);
//				Tfile << T << "\t";
//				Mfile << Q << "\t";
//			}
//			Tfile << endl;
//			Mfile << endl;
//		}
//
//		Tfile.close();
//		Mfile.close();
//	}
//}
//
//
//void ElectricPropulsion::compute(double V, double alt, double throttle){
//	V *= 3.28; // convert m/s to ft/s
//
//	if (V == prevV && alt == prevAlt && throttle == prevThrottle) return;
//
//	prevV = V;
//	prevAlt = alt;
//	prevThrottle = throttle;
//
//	motor->setThrottle(throttle);
//
//	double D = (prop->getRadius()/12.0)*2; // Prop diameter in feet
//	double rho = atm->getDensity(alt)*0.00194032033; // slugs/ft^3
//
//	double RPMmax = motor->getMaxRPM();
//	double RPMmin = 0;
//	T = 0;
//	Q = 0;
//	double Qmftlb = 0;
//	double eta_m,eta_p;
//
//	for (int i = 0; i < 10; i++){
//		double RPM = (RPMmax+RPMmin)/2.0;
//		double n = RPM/60;
//		double J = V/n/D;
//
//		eta_m = motor->getEfficiency(RPM);
//		double QinNm = motor->getTorque(RPM);
//
//		eta_p = prop->getEfficiency(J);
//		double CT = prop->getThrustCoefficient(J);
//		double CQ = prop->getTorqueCoefficient(J);
//
//		Qmftlb=0.7376*QinNm;
//
//		T = CT*rho*pow(n,2)*pow(D,4);
//		Q = CQ*rho*pow(n,2)*pow(D,5);
//
//		if (Qmftlb > Q){
//			RPMmin = RPM;
//		} else{
//			RPMmax = RPM;
//		}
//	}
//
//	//double PmonPp = Qmftlb/Q;
//
//	//double eff = eta_m*eta_p;
//
//	// multiply by number of motors
//	T *= motor->getNumberOfMotors();
//	T = max(0.0,T); // sometimes for low throttle setttings it doesn't quite converge and gives negative thrust
//
//	T *= 4.4482; // convert from lbf to N
//	Q *= 1.3558; // convert from lbf-ft to N-m
//}
